﻿
using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Text;

namespace RobotsSample
{
    /// <summary>
    /// 机器人示例抽象（含公共方法）
    /// </summary>
    public abstract class RobotSampleAbstract
    {

        #region 属性

        /// <summary>
        /// 绑定机器人算法的关联ID
        /// </summary>
        protected int RobotId { get; set; }

        /// <summary>
        /// 已连接的机器IP地址
        /// </summary>
        protected string ConnectedRobotIp { get; set; }

        /// <summary>
        /// 已连接的机器端口地址
        /// </summary>
        protected int ConnectedRobotPort { get; set; }

        /// <summary>
        /// 机器人的模块ID及模块所接的串口位置
        /// </summary>
        protected List<int> ModulesIds { get; private set; } = new List<int>();

        /// <summary>
        /// 关节模块及对应关节角度
        /// </summary>
        protected Dictionary<int, int> IdsNPostionAngles { get; private set; } = new Dictionary<int, int>();

        #endregion

        /// <summary>
        /// 运行
        /// </summary>
        public virtual void Run()
        {
            Console.WriteLine("Welcome to Robot Sample....");
            Console.WriteLine("Enter any key to scan robots ....");

            Console.ReadLine();

            List<Tuple<string, int, string>> robots = null;
            int robotsCount = 0;

            /*
             *######################################################################
             *####################### 机器人连接及模块扫描  ###########################
             *######################################################################
             */
            while (true)
            {
                //默认提示扫描机器人列表
                var cmd = 0;
                if (robotsCount > 0)
                {
                    Console.WriteLine("Enter the robot index (ex: 1)...[Input 0 to scan again]：\r\n");
                    var cmdStr = Console.ReadLine();
                    int.TryParse(cmdStr, out cmd);
                }

                if (cmd == 0)
                {
                    Console.WriteLine("Scaning the robots......\r\n");
                    robots = ScanOnlineRobots();
                    robotsCount = robots.Count;
                    Console.WriteLine($"Found robots:{robotsCount}");

                    for (var i = 0; i < robots.Count; i++)
                    {
                        Console.WriteLine($"{i + 1}：{robots[i].Item3}");
                    }
                }
                else if (cmd > 0 && cmd <= robotsCount)
                {
                    var cRobot = robots[cmd - 1];
                    Console.WriteLine($"Connecting----{cRobot.Item3}");

                    /*--连接机器人设备--*/
                    var b = ConnectRobot(cRobot.Item1, cRobot.Item2);
                    if (b)
                    {
                        ConnectedRobotIp = cRobot.Item1;
                        ConnectedRobotPort = cRobot.Item2;
                        Console.WriteLine("Connected");
                        break;
                    }
                    else
                    {
                        Console.WriteLine("Connect Failed");
                    }

                }
            }

            /*--扫描模块ID--*/
            var ids = ScanModulesIds();
            Console.WriteLine($"Modules Total Count：{ids.Count}");
            Console.WriteLine("Followed：");
            foreach (var id in ids)
            {
                Console.WriteLine($"Id:{id.Item1} Position:{id.Item2}");
            }
        }

        #region 保护方法

        /// <summary>
        /// 扫描当前在线机器人
        /// </summary>
        /// <returns>Item1:IP,Item2:端口,Item3：原始数据</returns>
        protected static List<Tuple<string, int, string>> ScanOnlineRobots()
        {
            var rs = new List<Tuple<string, int, string>>();
            var size = 100;

#if X64
            Int64[] ip = new Int64[size];
#else
            int[] ip = new int[size];
#endif
            int count = 0;
            IntPtr ips = IntArrayToIntptr(ip);
            var c = MiniBotControlLib.ScanRobots(out ips, ref count);

            //Console.WriteLine($"Found robots:{c}");
            for (int i = 0; i < count; i++)
            {
#if X64
                IntPtr ptr = new IntPtr(ips.ToInt64() + size * i);
#else
                IntPtr ptr = new IntPtr(ips.ToInt32() + size * i);
#endif

                var ipData = IntptrToArray(ptr, 100);
                string ipString = System.Text.Encoding.Default.GetString(ipData);
                if (!string.IsNullOrWhiteSpace(ipString))
                {
                    var connectInfo = ipString.Split(',');
                    if (connectInfo.Length < 3)
                    {
                        //数据不正确
                        System.Diagnostics.Debugger.Break();
                        continue;
                    }

                    var ipValue = connectInfo[0];
                    var portValue = 8899;

                    if (connectInfo[0].Contains(":"))
                    {
                        var values = connectInfo[0].Split(':');
                        ipValue = values[0];

                        int.TryParse(values[1], out portValue);
                    }

                    rs.Add(new Tuple<string, int, string>(ipValue, portValue, ipString));
                }

                //Console.WriteLine($"{i + 1}：{ipString}");
            }

            return rs;
        }

        /// <summary>
        /// 连接操作臂机器人
        /// </summary>
        /// <param name="ip"></param>
        /// <param name="port"></param>
        /// <returns></returns>
        protected bool ConnectRobot(string ip, int port)
        {
            var isConnected = MiniBotControlLib.Connect(ip, port) > 0;
            return isConnected;
        }

        /// <summary>
        /// 读取机器人ID列表
        /// </summary>
        /// <returns></returns>
        protected List<Tuple<int, int>> ScanModulesIds(byte[] defaultIds = null)
        {
            var rs = new List<Tuple<int, int>>();
            byte[] ids = new byte[30];
            byte[] pos = new byte[30];

            var iptId = ArrayToIntptr(ids);
            var iptPos = ArrayToIntptr(pos);
            var count = 0;


            #region 指定ID序列，一般用于测试

            if (defaultIds != null)
            {
                count = defaultIds.Length;
                for (var i = 0; i < defaultIds.Length; i++)
                {
                    ids[i] = defaultIds[i];
                }
            }
            else
            {
                count = MiniBotControlLib.ScanId(20, 49, iptId, iptPos);
            }
            #endregion


            for (var i = 0; i < count; i++)
            {
                rs.Add(new Tuple<int, int>(ids[i], pos[i]));
            }

            return rs;
        }

        /// <summary>
        /// 获取关节角度
        /// </summary>
        /// <param name="id"></param>
        /// <param name="angle"></param>
        protected int GetPosition(byte id)
        {
            return MiniBotControlLib.GetPosition(id);
        }

        /// <summary>
        /// 设置关节角度
        /// </summary>
        /// <param name="id"></param>
        /// <param name="angle"></param>
        protected void SetModulesPosition(byte id, double angle)
        {
            MiniBotControlLib.SetPosition(id, angle);
        }

        /// <summary>
        /// 向机器人发送指令
        /// </summary>
        /// <param name="robotId"></param>
        /// <param name="cmd"></param>
        /// <param name="parameter"></param>
        protected void SendCmd(int cmd, int parameter)
        {
            MiniBotAlgorithmLib.SendCmd(RobotId, cmd, parameter);
        }

        #endregion

        #region 硬件模块控制方法(用于执行运动学算法设置舵机参数的回调)

        /// <summary>
        /// MiniBotAlgorithmLib算法中设置关节角度的回调函数定义
        /// 参数：模块ID，角度（I:正负180；T：正负90；G：0到90）
        /// </summary>
        protected void SetPositionCallback(byte id, double angle)
        {
            MiniBotControlLib.SetPosition(id, angle);
        }

        /// <summary>
        /// MiniBotAlgorithmLib算法中以指定速度百分比设置关节角度的回调函数定义
        /// 参数：模块ID，角度（I:正负180；T：正负90；G：0到90），速度百分比
        /// </summary>
        protected void SetPositionWithSpeedCallback(byte id, double angle, int speedPercent)
        {
            MiniBotControlLib.SetPositionWithSpeed(id, angle, speedPercent);
        }

        /// <summary>
        /// MiniBotAlgorithmLib算法中设置多个关节角度的回调函数定义
        /// 参数：模块ID数组，角度数组，数组长度
        /// </summary>
        protected void SetMultiPositionCallback(IntPtr pids, IntPtr pangles, int count)
        {

            byte[] ids = IntptrToArray(pids, count);
            double[] angles = IntptrToDoubleArray(pangles, count);
            MiniBotControlLib.SetPositionSync(ids, angles, count);
        }


        /// <summary>
        /// MiniBotAlgorithmLib算法中以指定速度设置多个关节角度的回调函数定义
        /// 参数：模块ID数组，角度数组，数组长度，速度百分比
        /// </summary>
        protected void SetMultiPositionWithSpeedCallback(IntPtr pids, IntPtr pangles, int count, int speedPercent)
        {
            byte[] ids = IntptrToArray(pids, count);
            double[] angles = IntptrToDoubleArray(pangles, count);

            for (var i = 0; i < count; i++)
            {
                var id = ids[i];
                var angle = angles[i];
                MiniBotControlLib.SetPositionSpeed(id, speedPercent);
            }

            MiniBotControlLib.SetPositionSync(ids, angles, count);
        }

        /// <summary>
        /// MiniBotAlgorithmLib算法中设置关节速度的回调函数定义
        /// 参数：模块ID，速度百分比（0到100）
        /// </summary>
        protected void SetPositionSpeedCallback(byte id, int speedPercent)
        {
            MiniBotControlLib.SetPositionSpeed(id, speedPercent);
        }

        /// <summary>
        /// MiniBotAlgorithmLib算法中设置关节运行速度的回调函数定义
        /// 参数：模块ID，速度百分比（0到100）
        /// </summary>
        protected void SetRunningSpeedCallback(byte id, int speedPercent)
        {
            MiniBotControlLib.SetRunningSpeed(id, speedPercent);
        }

        /// <summary>
        /// MiniBotAlgorithmLib算法中设置关节是否使能的回调函数定义
        /// 参数：模块ID，是否使能（转矩是否生效）
        /// </summary>
        protected void SetTorqueEnableCallback(byte id, bool isEnable)
        {
            MiniBotControlLib.EnableTorque(id, isEnable);
        }

        /// MiniBotAlgorithmLib算法中设置模块运行模式的回调函数定义
        /// 参数：模块ID，模式值(0:位置模式；1:速度模式；2:PWM开环调速度模式；)
        /// </summary>
        protected void SetModeCallback(byte id, int mode)
        {
            MiniBotControlLib.SetMotorMode(id, mode);
        }

        #endregion

        #region 静态公共方法

        /// <summary>
        /// 将字节数组转换成指针
        /// </summary>
        /// <param name="source"></param>
        /// <returns></returns>
        internal static IntPtr ArrayToIntptr(byte[] source)
        {
            if (source == null)
                return IntPtr.Zero;
            unsafe
            {
                fixed (byte* point = source)
                {
                    IntPtr ptr = new IntPtr(point);
                    return ptr;
                }
            }
        }

        /// <summary>
        /// 将整型数组转换成指针
        /// </summary>
        /// <param name="source"></param>
        /// <returns></returns>
        internal static IntPtr IntArrayToIntptr(int[] source)
        {
            if (source == null)
                return IntPtr.Zero;
            unsafe
            {
                fixed (int* point = source)
                {
                    IntPtr ptr = new IntPtr(point);
                    return ptr;
                }
            }
        }

        /// <summary>
        /// 将64位整型数组转换成指针
        /// </summary>
        /// <param name="source"></param>
        /// <returns></returns>
        internal static IntPtr IntArrayToIntptr(Int64[] source)
        {
            if (source == null)
                return IntPtr.Zero;
            unsafe
            {
                fixed (Int64* point = source)
                {
                    IntPtr ptr = new IntPtr(point);
                    return ptr;
                }
            }
        }

        /// <summary>
        /// 将浮点数组转换成指针
        /// </summary>
        /// <param name="source"></param>
        /// <returns></returns>
        internal static IntPtr FloatArrayToIntptr(float[] source)
        {
            if (source == null)
                return IntPtr.Zero;
            unsafe
            {
                fixed (float* point = source)
                {
                    IntPtr ptr = new IntPtr(point);
                    return ptr;
                }
            }
        }




        /// <summary>
        /// 将指针转为字节数组
        /// </summary>
        /// <param name="intPtr"></param>
        /// <param name="size"></param>
        /// <returns></returns>
        protected static byte[] IntptrToArray(IntPtr intPtr, int size)
        {
            byte[] resultData = new byte[size];
            Marshal.Copy(intPtr, resultData, 0, size);
            return resultData;
        }

        /// <summary>
        /// 将指针转为浮点数组
        /// </summary>
        /// <param name="intPtr"></param>
        /// <param name="size"></param>
        /// <returns></returns>
        protected static float[] IntptrToFloatArray(IntPtr intPtr, int size)
        {
            float[] resultData = new float[size];
            Marshal.Copy(intPtr, resultData, 0, size);
            return resultData;
        }

        /// <summary>
        /// 将指针转为双精度浮点数组
        /// </summary>
        /// <param name="intPtr"></param>
        /// <param name="size"></param>
        /// <returns></returns>
        protected static double[] IntptrToDoubleArray(IntPtr intPtr, int size)
        {
            double[] resultData = new double[size];
            Marshal.Copy(intPtr, resultData, 0, size);
            return resultData;
        }




        #endregion
    }
}
